Description

This is was a stacked PR: #3756 #3758 #3759 #3760.

This PR implements the new primitive executor logic, specifically:

• When being sent a new move primitive, checks to see trajectory destination is different before switching to it.
• Uses position/orientation controllers (which themselves use pid) to get the target linear/angular velocity

There are some necessary changes in this PR that are not in prim exec:

• New hasSameDestination() method for trajectory classes
• Extra param to generate trajectory with a new start point / start angle
• Bugfix where erforce simulator passes in non-inverted worlds to yellow team primitive executors (supposed to be inverted)

Testing Done

Resolved Issues

Length Justification and Key Files to Review

Review Checklist

It is the reviewers responsibility to also make sure every item here has been covered

• Function & Class comments: All function definitions (usually in the .h file) should have a javadoc style comment at the start of them. For examples, see the functions defined in thunderbots/software/geom. Similarly, all classes should have an associated Javadoc comment explaining the purpose of the class.
• Remove all commented out code
• Remove extra print statements: for example, those just used for testing
• Resolve all TODO's: All TODO (or similar) statements should either be completed or associated with a github issue